/*********************************************************************
* Authors: CJH
*********************************************************************/
#include <angles/angles.h>
#include <IPPNS_1034/bsplines_global_planner.h>
#include <pluginlib/class_list_macros.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2/convert.h>
#include <tf2/utils.h>



//MarkArray type mark
#define MARKARROW 0//箭头
#define MARKSPHERE 2//球
#define MARKTEXT_VIEW_FACING 9//显示3D的文字

//MarkArray action
#define MAADD 0 //MarkerArray add
#define MADEL 1 //MarkerArray delete

//MarkArray form limit
#define SCALEX_MAX 4.0f //MAX scale length
#define SCALEY_MAX 0.3f //MAX scale width
#define SCALEX_MIN 0.05f //MIN scale length
#define SCALEY_MIN 0.2f //MIN scale width



//register this planner as a BaseGlobalPlanner plugin
PLUGINLIB_EXPORT_CLASS(bsplines_planner::Bsplines_planner, nav_core::BaseGlobalPlanner)

namespace bsplines_planner {

  Bsplines_planner::Bsplines_planner()
  : initialized_(false){}

  Bsplines_planner::Bsplines_planner(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
  :initialized_(false){
    initialize(name, costmap_ros);
  }

  Bsplines_planner::~Bsplines_planner() {
    // deleting a nullptr is a noop
  }
  
  void Bsplines_planner::Markerinit(const int action,const int type,const float scale){
    std::default_random_engine rand_engine;//生成随机数，为mark附上颜色
    std::uniform_real_distribution<double> RED(0,1);// 左闭右闭区间;
    std::uniform_real_distribution<double> GREEN(0,1);
    std::uniform_real_distribution<double> BLUE(0,1);
    rand_engine.seed(time(0));
    switch (type)
    {
    case MARKSPHERE:
      costArray.ns="waypoint_MARKSPHERE";//命名空间namespace
      costArray.type=type;//类型
      costArray.scale.x = scale>SCALEX_MIN ? scale:SCALEX_MIN;
      costArray.scale.y= scale>SCALEX_MIN ? scale:SCALEX_MIN;
      costArray.pose.orientation.x = 0;
      costArray.pose.orientation.y = 0;
      costArray.pose.orientation.z = 0;
      costArray.pose.orientation.w = 1;
      costArray.scale.z= 0.01;
      costArray.color.r = RED(rand_engine);
      costArray.color.g = GREEN(rand_engine);
      costArray.color.b = BLUE(rand_engine);
      costArray.color.a = (action==MAADD ? 0.7 :0);

      break;
    case MARKARROW:
      costArray.ns="waypoint_MARKARROW";//命名空间namespace
      costArray.type=type;//类型
      costArray.scale.x = scale;//长度
      costArray.scale.y = scale < SCALEY_MAX ? (scale/SCALEX_MAX):SCALEY_MAX;//不能让箭头宽度超过，限制宽度
      costArray.scale.z = 0.01;//不能让箭头宽度超过，限制宽度
      costArray.color.r = RED(rand_engine);
      costArray.color.g = GREEN(rand_engine);
      costArray.color.b = BLUE(rand_engine);
      costArray.color.a = (action==MAADD ? ((scale>SCALEX_MIN ? scale:SCALEX_MIN)/SCALEX_MAX):0);
      break;
    }
  }


  //通过MarkArray显示每个路径点
  void Bsplines_planner::MarkerArrayprint(const int action,const int type){
    int count_id=0;
    //不能使用范围循环，否则无法得到下一个值，在这构造双指针
    auto pos = this->Waypoints_list.begin();//慢指针
    auto pos_next = this->Waypoints_list.begin();//快指针
    for (pos;pos!=this->Waypoints_list.end();pos++){
      const auto &element_current =*pos;
      //解决list的越界问题，且size=1时，依旧begin!=end
      const auto &element_next=*(count_id < this->Waypoints_list.size()-1 ? ++pos_next:pos);//pos的增加慢于id，则返回原pos否则返回++p,先增再取
      //设定MarkerArray的一些性质
      costArray.id=count_id;
      costArray.header.frame_id=element_current->header.frame_id;
      costArray.header.stamp=ros::Time::now();
      switch (type)
      {
        case MARKSPHERE:
          //为每个waypoint赋值上pose信息
          costArray.pose.position.x =element_current->point.x;
          costArray.pose.position.y =element_current->point.y;
          costArray.lifetime=ros::Duration();
          //对每个waypoint进行MarkArray上色
          this->Markerinit(action,type,0.2);
          count_id++;
          //添加进markers数组
          costArrays.markers.push_back(costArray);
        break;

        case MARKARROW:
          
          Point2f point_start(element_current->point.x,element_current->point.y);
          Point2f point_end(element_next->point.x,element_next->point.y);
          // std::cout<<"point_start"<<point_start<<std::endl;
          // std::cout<<"point_end"<<point_end<<std::endl;
          //为每个waypoint赋值上pose信息
          costArray.pose.position.x =point_start[0];
          costArray.pose.position.y =point_start[1];
           
          double dis = (point_start-point_end).norm();
          double yaw= atan2((point_end[1]-point_start[1]), (point_end[0]-point_start[0]));
          // std::cout<<"yaw"<<yaw<<std::endl;

          tf2::Quaternion qtn;
          qtn.setRPY(0,0,yaw);//转换为四元数
          costArray.pose.orientation.x = qtn.getX();
          costArray.pose.orientation.y = qtn.getY();
          costArray.pose.orientation.z = qtn.getZ();
          costArray.pose.orientation.w = qtn.getW();
          costArray.lifetime=ros::Duration();
          //对每个waypoint进行MarkArray上色
          this->Markerinit(action,type,dis);
          count_id++;
          //添加进markers数组
          costArrays.markers.push_back(costArray);
        break;
      }
    }
    this->Pub_.publish(costArrays); 
    costArrays.markers.clear();
    ros::Duration(0.001).sleep();//设置1ms的延迟
  }




  /*路径点向输入双向队列输入的接口，保留点击这个点的时候可以为其更改数值的操作接口:
	param:1.任意增添路径点
		2.删改当前路径点
		3.任意插入路径点
		4.一次性删除所有路径点
		5.查看当前的路径点
		6.更改当前路径点的信息
	*/

	/*1.任意增添路径点*/
	void Bsplines_planner::waypoint_add(const geometry_msgs::PointStampedConstPtr &newpoint){
		//插入新的路径点
		//todo 没有连续输入的接口，后续优化，现为每次单次输入
		if (newpoint!=NULL){
		this->Waypoints_list.push_back(newpoint);
		}//如果新的路径点已被输入不是空指针
	}

  /*2.任意删改路径点*/
	void Bsplines_planner::waypoint_delete(){
		//easy to operate,直接删除最后一个路径点
		this->Waypoints_list.erase(this->Waypoints_list.end());
	}


  /*4.一次性删除所有路径点*/
	void Bsplines_planner::waypoint_clear(){
		static std::string clear_flag;
		ROS_WARN("Ready to clear all list:");
		std::cout<<"[y or n]"<<std::endl; 
		std::cin>>clear_flag;
		if (clear_flag=="y"){
      //清除路径，并清除全局地图
			this->Waypoints_list.clear();
			ROS_WARN("Successfully clear all list");
		}
	}

  void Bsplines_planner::waypointCallback(const geometry_msgs::PointStampedConstPtr& waypoint){
      ROS_INFO("pose show start!");
      //将新的waypoint push_back to Waypoints_list
      MoveBaseClient ac("move_base",true);
      //doing_为true证明执行过了，makeplan，再次取点的时候，需要重新清空
      if (doing_){
        //清除路径
        this->waypoint_clear();
        
        //通过MADEL来清除路径点
        this->MarkerArrayprint(MADEL,MARKSPHERE);
        this->MarkerArrayprint(MADEL,MARKARROW);
        ac.cancelAllGoals();//当需要重新设置点时，终止导航任务
        doing_=false;
      }
      else if (!doing_)
      {
        this->waypoint_add(waypoint);
        this->MarkerArrayprint(MAADD,MARKSPHERE);//用于画点
        this->MarkerArrayprint(MAADD,MARKARROW);//多于等于两个点，用于画箭头
      }
  }
  
  void Bsplines_planner::pointListener(){
    ros::Rate loop_rate(10);
    //订阅pulish point节点得到
    //初始化订阅节点与发布节点
    this->Pub_= nh.advertise<visualization_msgs::MarkerArray>("waypoint", 10);/*定义点的性质*/
    while (ros::ok())//代替spin()，避免出现多重spinner的阻塞问题
    {     
      //使用boost::bind绑定回调函数，_1是占为符，表示这个PointStamped msg
      Sub_=nh.subscribe<geometry_msgs::PointStamped>("/clicked_point",1000,boost::bind(&Bsplines_planner::waypointCallback,this,_1));
      loop_rate.sleep();
    }
  }


  void Bsplines_planner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
    if(!initialized_){
      //创建对象内，监听路径点的线程,通过boost绑定对象内函数进行调用
      std::thread listener(boost::bind(&Bsplines_planner::pointListener,this));
      listener.detach();//通过detach()的方法进行调用，从而不影响其他程序的进行 
      ros::NodeHandle private_nh("~/" + name);
      private_nh.param("stride",stride,0.02);
      //设置可视化地图选项
      name_=name;
      initialized_ = true;
      doing_=false;
      this->Pub_path_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
    }
    else
      ROS_WARN("This planner has already been initialized... doing nothing");
  }




  //Regardless of whether the path points are in the cost map, only the points of the route planning are considered, and approximate fitting or interpolation fitting is adopted
  void Bsplines_planner::routeFitting(const geometry_msgs::PoseStamped& start, 
      const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
      //加入到plan的新点
      geometry_msgs::PoseStamped new_goal=goal;
      //用于计算插值角度的前后值
      Point2f current_point,last_point;
      //开始末尾的构造点
      Point2f start_point2d(start.pose.position.x,start.pose.position.y);
      Point2f end_point2d(goal.pose.position.x,goal.pose.position.y);
      std::vector<Point2f> discretePoints{start_point2d,start_point2d,start_point2d,end_point2d,end_point2d,end_point2d,end_point2d};//补齐三个起点与终点
      //先装载启始和末尾的点，其余三次曲线拟合反推四个控制点，再计算单个离散点
      for (const auto& element : this->Waypoints_list){
        Point2f coord_xy(element->point.x,element->point.y);
          discretePoints.insert(discretePoints.end()-4,coord_xy);//在最后一个前插入
		  }
      //先载入起始点,避免无法拟合起始路线
      plan.push_back(start);
      //可视化地图的载入
      std::cout<<"start:"<<start<<std::endl;
      for (int i=0;i<discretePoints.size()-4;i++){
        Point2f xy[4];
        xy[0] = (discretePoints[i] + 4 * discretePoints[(i + 1) % discretePoints.size()] + discretePoints[(i + 2) % discretePoints.size()]) / 6;
        xy[1] = -(discretePoints[i] - discretePoints[(i + 2) % discretePoints.size()]) / 2;
        xy[2] = (discretePoints[i] - 2 * discretePoints[(i + 1) % discretePoints.size()] + discretePoints[(i + 2) % discretePoints.size()]) / 2;
        xy[3] = -(discretePoints[i] - 3 * discretePoints[(i + 1) % discretePoints.size()] + 3 * discretePoints[(i + 2) % discretePoints.size()] - discretePoints[(i + 3) % discretePoints.size()]) / 6;
        for (double t = 0; t <= 1; t += stride) {
          Point2f current_point = Point2f(0, 0);
          for (int j = 0; j < 4; j++) {
            current_point += xy[j] * pow(t, j);
          }
        //push上一个点的信息,才可以计算角度,因此从第一个点后才开始push
        if(i!=0){
          double yaw= atan2((current_point[1]-last_point[1]), (current_point[0]-last_point[0]));
          // std::cout<<"yaw"<<yaw<<std::endl;
          new_goal.pose.position.x=last_point[0];
          new_goal.pose.position.y=last_point[1];
          tf2::Quaternion qtn;
          qtn.setRPY(0,0,yaw);//转换为四元数
          new_goal.pose.orientation.x = qtn.getX();
          new_goal.pose.orientation.y = qtn.getY();
          new_goal.pose.orientation.z = qtn.getZ();
          new_goal.pose.orientation.w = qtn.getW();
          plan.push_back(new_goal);
          }
        last_point=current_point;
        }
      }
      plan.push_back(goal);
      std::cout<<"goal:"<<goal<<std::endl;
  }

  void Bsplines_planner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path) {
    if (!initialized_) {
        ROS_ERROR(
                "This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return;
    }
    //create a message for the plan
    nav_msgs::Path gui_path;
    gui_path.poses.resize(path.size());

    gui_path.header.frame_id = "map";
    gui_path.header.stamp = ros::Time::now();

    // Extract the plan in world co-ordinates, we assume the path is all in the same frame
    for (unsigned int i = 0; i < path.size(); i++) {
        gui_path.poses[i] = path[i];
    }

    this->Pub_path_.publish(gui_path);
}




  bool Bsplines_planner::makePlan(const geometry_msgs::PoseStamped& start, 
      const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){

    if(!initialized_){
      ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
      return false;
    }
    
    plan.clear();

    
    try
    {
      if (!doing_){
        //当进行巡航时，doing_会拉高表示正在运行
        start_=start;//doing_为了固定起点，不然起点是响应时的当前点位，只有当重新发布终点时，才更改起点
        doing_=true;//doing_为true证明执行过了，makeplan，再次取点的时候，需要重新清空
      }
      this->routeFitting(start_,goal,plan);
      this->publishPlan(plan);
      //发布可视化地图
    }
    catch(const std::exception& e)
    {
      std::cerr << e.what() << '\n';
    }
    
    return true;
  }

};
